#ifndef SENSOR_H
#define SENSOR_H

#include <Wire.h>
#include <math.h>
#include <Arduino.h>

class AS5600
{
private:
    uint8_t device_address;
    uint8_t high_address;
    uint8_t low_address;
    double angle_buffer;
    double init_angle;
    bool read_angle_loop_flag;
    TaskHandle_t *read_angle_handlep;
    QueueHandle_t mutex_handle;

    double _normalize_angle(double angle)
    {
        angle = fmod(angle, 2 * PI);
        return (angle < 0) ? angle + 2 * PI : angle;
    }

    static void _read_angle_loop_wrapper(void *arg)
    {
        AS5600 *asp = static_cast<AS5600 *>(arg);
        asp->_read_angle_loop();
    }

    uint8_t _read_register(uint8_t device_address, uint8_t register_address);
    void _read_angle_loop();

public:
    AS5600(int sda = -1, int scl = -1, uint32_t frequency = 400000);
    void set_init_angle();
    void read_angle();
    void read_angle_task_start();
    void read_angle_task_end();
    double get_angle();
};

#endif